#include "utility.h"
#include "dcl_lio_sam/cloud_info.h" //不太清楚这里的两个。h文件是否是参数文件中的设置
#include "dcl_lio_sam/save_map.h"
#include "distributedMapping.h"

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>

#include <gtsam/nonlinear/ISAM2.h>

using namespace gtsam;

using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::G; // GPS pose
using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)

/*
 * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
 */
// 构建一个点云数据结构，包含6D位姿信息（[x,y,z,roll,pitch,yaw] intensity is time stamp）
struct PointXYZIRPYT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding 添加xyz
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRPYT,
                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, roll, roll)(float, pitch, pitch)(float, yaw, yaw)(double, time, time))

typedef PointXYZIRPYT PointTypePose;

// ParamServer基类
class mapOptimization : public ParamServer
{

public:
    // gtsam
    NonlinearFactorGraph gtSAMgraph; // 调用gtsam的非线性因子图
    Values initialEstimate;          // 声明了一个Values的存储初始估计值，通过Values构建一个非结构化的因子图
    Values optimizedEstimate;        // 同理，构建优化后的估计值
    ISAM2 *isam;                     // 求解器
    Values isamCurrentEstimate;      // 用于计算一个估计值并获取最新的估计结果。
    Eigen::MatrixXd poseCovariance;  // 构建名为poseCovariance的动态矩阵，用于存储位姿协方差矩阵

    distributedMapping dm; // 构建分布地图映射对象

    ros::Publisher pubLaserCloudSurround;
    ros::Publisher pubLaserOdometryGlobal;
    ros::Publisher pubLaserOdometryIncremental; // 发布雷达里程计增量
    ros::Publisher pubKeyPoses;
    ros::Publisher pubPath;

    ros::Publisher pubHistoryKeyFrames;
    ros::Publisher pubIcpKeyFrames;
    ros::Publisher pubRecentKeyFrames;
    ros::Publisher pubRecentKeyFrame;
    ros::Publisher pubCloudRegisteredRaw;
    ros::Publisher pubLoopConstraintEdge;

    // 接收者，接收点云信息、GPS信息、闭环信息
    ros::Subscriber subCloud;
    ros::Subscriber subGPS;
    ros::Subscriber subLoop;

    // 提供者，提供保存地图的服务
    ros::ServiceServer srvSaveMap;

    std::deque<nav_msgs::Odometry> gpsQueue;
    dcl_lio_sam::cloud_info cloudInfo;

    vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
    vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;

    pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
    pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
    // 复制点云位姿
    pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;
    pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;

    pcl::PointCloud<PointType>::Ptr laserCloudCornerLast;   // 点特征点云数据集 corner feature set from odoOptimization
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLast;     // 曲面特征点云数据集surf feature set from odoOptimization
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // 下采样版本 downsampled corner feature set from odoOptimization
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS;   // 下采样版本 downsampled surf feature set from odoOptimization

    pcl::PointCloud<PointType>::Ptr laserCloudOri;
    pcl::PointCloud<PointType>::Ptr coeffSel;

    std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
    std::vector<PointType> coeffSelCornerVec;
    std::vector<bool> laserCloudOriCornerFlag;
    std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
    std::vector<PointType> coeffSelSurfVec;
    std::vector<bool> laserCloudOriSurfFlag;
    // 构建一个map类型，使用int索引，存储多个点云对象
    map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;

    // KD树的构建
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;

    // 相邻关键帧，使用了点云库（pcl）中的KdTreeFLANN类，它是一种用于快速最近邻搜索的数据结构。
    // 存储周围关键位姿的kd树。这个kd树可以用于更快地执行搜索和匹配操作。
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
    // 存储历史关键帧的kd树
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
    // 滤波器
    pcl::VoxelGrid<PointType> downSizeFilterCorner;
    pcl::VoxelGrid<PointType> downSizeFilterSurf;
    pcl::VoxelGrid<PointType> downSizeFilterICP;
    pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
    // 当前帧点云时间戳
    ros::Time timeLaserInfoStamp;
    double timeLaserInfoCur;

    float transformTobeMapped[6];

    std::mutex mtx;
    std::mutex mtxLoopInfo;

    bool isDegenerate = false;
    cv::Mat matP;

    int laserCloudCornerFromMapDSNum = 0;
    int laserCloudSurfFromMapDSNum = 0;
    int laserCloudCornerLastDSNum = 0;
    int laserCloudSurfLastDSNum = 0;

    bool aLoopIsClosed = false;
    map<int, int> loopIndexContainer;                               // from new to old存储从新到旧的循环索引
    vector<pair<int, int>> loopIndexQueue;                          // 存储循环索引队列。
    vector<gtsam::Pose3> loopPoseQueue;                             // 存储循环位姿队列。
    vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue; // 存储循环噪声队列。
    deque<std_msgs::Float64MultiArray> loopInfoVec;                 // 存储循环信息向量。

    nav_msgs::Path globalPath;

    Eigen::Affine3f transPointAssociateToMap;
    Eigen::Affine3f incrementalOdometryAffineFront;
    Eigen::Affine3f incrementalOdometryAffineBack;

    mapOptimization()
    {
        // 定义ISAM2参数类(ISAM2Params类)对象parameters
        ISAM2Params parameters;

        // 重线性化阈值relinearizeThreshold = 0.1
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 1; // 线性化频率
        isam = new ISAM2(parameters);

        pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
        pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
        pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry>("lio_sam/mapping/odometry", 1);
        pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 1);
        pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);

        subCloud = nh.subscribe<dcl_lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
        subGPS = nh.subscribe<nav_msgs::Odometry>(gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
        // subLoop  = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());

        // srvSaveMap  = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);

        pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
        pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);
        pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/lio_sam/mapping/loop_closure_constraints", 1);

        pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);
        pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
        pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);

        downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
        downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
        downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
        downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization

        allocateMemory();
    }

    void allocateMemory()
    {
        cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
        cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
        copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
        copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());

        kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
        kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());

        laserCloudCornerLast.reset(new pcl::PointCloud<PointType>());   // corner feature set from odoOptimization
        laserCloudSurfLast.reset(new pcl::PointCloud<PointType>());     // surf feature set from odoOptimization
        laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
        laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>());   // downsampled surf featuer set from odoOptimization

        laserCloudOri.reset(new pcl::PointCloud<PointType>());
        coeffSel.reset(new pcl::PointCloud<PointType>());

        laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
        coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
        coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);

        std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);

        laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());

        kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
        kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());

        for (int i = 0; i < 6; ++i)
        {
            transformTobeMapped[i] = 0;
        }

        matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
    }
    // 当接收到提取好特征的点云消息,就会调用该函数
    void laserCloudInfoHandler(const dcl_lio_sam::cloud_infoConstPtr &msgIn)
    {
        // extract time stamp
        // 获取点云信息消息的时间戳(ros::Time类)timeLaserInfoStamp和时间点timeLaserInfoCur(double)
        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserInfoCur = msgIn->header.stamp.toSec();

        // extract info and feature cloud
        cloudInfo = *msgIn;
        pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
        pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);

        std::lock_guard<std::mutex> lock(mtx);
        // 定义静态变量timeLastProcessing 初始化为-1，用来记录【上一次】处理点云信息消息的时间
        static double timeLastProcessing = -1;
        // 如果【当前】点云信息消息的时间点timeLaserInfoCur距离【上一次】处理点云信息消息的时间超过0.15s
        // 假设上一次点云时间timeLastProcessing 0.1s，则当前点云时间戳0.2s不满足，要下一帧点云时间戳0.3s才满足，
        // 才能进入if判断，然后将timeLastProcessing更新为0.3s
        if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
        {
            dm.unlockOnCall(); // 解锁
            timeLastProcessing = timeLaserInfoCur;

            updateInitialGuess();

            extractSurroundingKeyFrames();

            downsampleCurrentScan();

            scan2MapOptimization();

            /* dcl-slam */
            // assign scan2map incremental key pose
            //  GTSAM 库中的 Pose3 类来创建一个表示姿态，使用三个旋转角度和三个平移向量。
            gtsam::Pose3 pose_to = Pose3(
                Rot3::RzRyRx(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
                Point3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
            // 拷贝全局路径
            nav::Path global_path = dm.getGlobalPath();


            if (dm.saveFrame(pose_to) == true)
            {
                // keyframe关键帧容器
                pcl::PointCloud<pcl::PointXYZI>::Ptr keyframe(new pcl::PointCloud<pcl::PointXYZI>());
                pcl::fromROSMsg(msgIn->cloud_deskewed, *keyframe);

                dm.performDistributedMapping(pose_to, keyframe, timeLaserInfoStamp);

                if (dm.updatePoses())
                {
                    // clear map cache
                    // 构建一个map类型，使用int索引，存储多个点云对象
                    laserCloudMapContainer.clear();
                }
                // save updated transform
                // 最新的位姿估计，赋值给transformTobeMapped向量
                gtsam::Pose3 latest_estimate = dm.getLatestEstimate();
                transformTobeMapped[0] = latest_estimate.rotation().roll();
                transformTobeMapped[1] = latest_estimate.rotation().pitch();
                transformTobeMapped[2] = latest_estimate.rotation().yaw();
                transformTobeMapped[3] = latest_estimate.translation().x();
                transformTobeMapped[4] = latest_estimate.translation().y();
                transformTobeMapped[5] = latest_estimate.translation().z();

                dm.makeDescriptors();

                dm.publishPath();

                // save all the received edge and surf points
                pcl::PointCloud<PointType>::Ptr keyframe_corner(new pcl::PointCloud<PointType>());
                pcl::PointCloud<PointType>::Ptr keyframe_surf(new pcl::PointCloud<PointType>());
                pcl::copyPointCloud(*laserCloudCornerLastDS, *keyframe_corner);
                pcl::copyPointCloud(*laserCloudSurfLastDS, *keyframe_surf);

                // save key frame cloud
                cornerCloudKeyFrames.push_back(keyframe_corner);
                surfCloudKeyFrames.push_back(keyframe_surf);
            }
            dm.publishTransformation(timeLaserInfoStamp);
            dm.lockOnCall();
            /* dcl-slam */
            // 保存关键帧的因子
            saveKeyFramesAndFactor();

            correctPoses();

            publishOdometry();

            publishFrames();
        }
    }

    void gpsHandler(const nav_msgs::Odometry::ConstPtr &gpsMsg)
    {
        gpsQueue.push_back(*gpsMsg);
    }

    void pointAssociateToMap(PointType const *const pi, PointType *const po)
    {
        po->x = transPointAssociateToMap(0, 0) * pi->x + transPointAssociateToMap(0, 1) * pi->y + transPointAssociateToMap(0, 2) * pi->z + transPointAssociateToMap(0, 3);
        po->y = transPointAssociateToMap(1, 0) * pi->x + transPointAssociateToMap(1, 1) * pi->y + transPointAssociateToMap(1, 2) * pi->z + transPointAssociateToMap(1, 3);
        po->z = transPointAssociateToMap(2, 0) * pi->x + transPointAssociateToMap(2, 1) * pi->y + transPointAssociateToMap(2, 2) * pi->z + transPointAssociateToMap(2, 3);
        po->intensity = pi->intensity;
    }

    pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose *transformIn)
    {
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

        int cloudSize = cloudIn->size();
        cloudOut->resize(cloudSize);

        Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);

#pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < cloudSize; ++i)
        {
            const auto &pointFrom = cloudIn->points[i];
            cloudOut->points[i].x = transCur(0, 0) * pointFrom.x + transCur(0, 1) * pointFrom.y + transCur(0, 2) * pointFrom.z + transCur(0, 3);
            cloudOut->points[i].y = transCur(1, 0) * pointFrom.x + transCur(1, 1) * pointFrom.y + transCur(1, 2) * pointFrom.z + transCur(1, 3);
            cloudOut->points[i].z = transCur(2, 0) * pointFrom.x + transCur(2, 1) * pointFrom.y + transCur(2, 2) * pointFrom.z + transCur(2, 3);
            cloudOut->points[i].intensity = pointFrom.intensity;
        }
        return cloudOut;
    }

    pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointPose6D *transformIn)
    {
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

        int cloudSize = cloudIn->size();
        cloudOut->resize(cloudSize);

        Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);

#pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < cloudSize; ++i)
        {
            const auto &pointFrom = cloudIn->points[i];
            cloudOut->points[i].x = transCur(0, 0) * pointFrom.x + transCur(0, 1) * pointFrom.y + transCur(0, 2) * pointFrom.z + transCur(0, 3);
            cloudOut->points[i].y = transCur(1, 0) * pointFrom.x + transCur(1, 1) * pointFrom.y + transCur(1, 2) * pointFrom.z + transCur(1, 3);
            cloudOut->points[i].z = transCur(2, 0) * pointFrom.x + transCur(2, 1) * pointFrom.y + transCur(2, 2) * pointFrom.z + transCur(2, 3);
            cloudOut->points[i].intensity = pointFrom.intensity;
        }
        return cloudOut;
    }

    gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint)
    {
        return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
                            gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
    }

    gtsam::Pose3 trans2gtsamPose(float transformIn[])
    {
        return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
                            gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
    }

    Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint)
    {
        return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
    }

    Eigen::Affine3f trans2Affine3f(float transformIn[])
    {
        return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
    }

    PointTypePose trans2PointTypePose(float transformIn[])
    {
        PointTypePose thisPose6D;
        thisPose6D.x = transformIn[3];
        thisPose6D.y = transformIn[4];
        thisPose6D.z = transformIn[5];
        thisPose6D.roll = transformIn[0];
        thisPose6D.pitch = transformIn[1];
        thisPose6D.yaw = transformIn[2];
        return thisPose6D;
    }

    // bool saveMapService(dcl_lio_sam::save_mapRequest& req, dcl_lio_sam::save_mapResponse& res)
    // {
    //   string saveMapDirectory;

    //   cout << "****************************************************" << endl;
    //   cout << "Saving map to pcd files ..." << endl;
    //   if(req.destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory;
    //   else saveMapDirectory = std::getenv("HOME") + req.destination;
    //   cout << "Save destination: " << saveMapDirectory << endl;
    //   // create directory and remove old files;
    //   int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str());
    //   unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());
    //   // save key frame transformations
    //   pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);
    //   pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);
    //   // extract global point cloud map
    //   pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
    //   pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());
    //   pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
    //   pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());
    //   pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
    //   for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {
    //       *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i],  &cloudKeyPoses6D->points[i]);
    //       *globalSurfCloud   += *transformPointCloud(surfCloudKeyFrames[i],    &cloudKeyPoses6D->points[i]);
    //       cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
    //   }

    //   if(req.resolution != 0)
    //   {
    //     cout << "\n\nSave resolution: " << req.resolution << endl;

    //     // down-sample and save corner cloud
    //     downSizeFilterCorner.setInputCloud(globalCornerCloud);
    //     downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution);
    //     downSizeFilterCorner.filter(*globalCornerCloudDS);
    //     pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);
    //     // down-sample and save surf cloud
    //     downSizeFilterSurf.setInputCloud(globalSurfCloud);
    //     downSizeFilterSurf.setLeafSize(req.resolution, req.resolution, req.resolution);
    //     downSizeFilterSurf.filter(*globalSurfCloudDS);
    //     pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS);
    //   }
    //   else
    //   {
    //     // save corner cloud
    //     pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);
    //     // save surf cloud
    //     pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud);
    //   }

    //   // save global point cloud map
    //   *globalMapCloud += *globalCornerCloud;
    //   *globalMapCloud += *globalSurfCloud;

    //   int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud);
    //   res.success = ret == 0;

    //   downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
    //   downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);

    //   cout << "****************************************************" << endl;
    //   cout << "Saving map to pcd files completed\n" << endl;

    //   return true;
    // }

    // void visualizeGlobalMapThread()
    // {
    //     ros::Rate rate(0.2);
    //     while (ros::ok()){
    //         rate.sleep();
    //         publishGlobalMap();
    //     }

    //     if (savePCD == false)
    //         return;

    //     dcl_lio_sam::save_mapRequest  req;
    //     dcl_lio_sam::save_mapResponse res;

    //     if(!saveMapService(req, res)){
    //         cout << "Fail to save map" << endl;
    //     }
    // }

    // void publishGlobalMap()
    // {
    //     if (pubLaserCloudSurround.getNumSubscribers() == 0)
    //         return;

    //     if (cloudKeyPoses3D->points.empty() == true)
    //         return;

    //     pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());;
    //     pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());
    //     pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());
    //     pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());
    //     pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());

    //     // kd-tree to find near key frames to visualize
    //     std::vector<int> pointSearchIndGlobalMap;
    //     std::vector<float> pointSearchSqDisGlobalMap;
    //     // search near key frames to visualize
    //     mtx.lock();
    //     kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
    //     kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
    //     mtx.unlock();

    //     for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
    //         globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
    //     // downsample near selected key frames
    //     pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualization
    //     downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
    //     downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
    //     downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
    //     for(auto& pt : globalMapKeyPosesDS->points)
    //     {
    //         kdtreeGlobalMap->nearestKSearch(pt, 1, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap);
    //         pt.intensity = cloudKeyPoses3D->points[pointSearchIndGlobalMap[0]].intensity;
    //     }

    for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
        globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
    // downsample near selected key frames
    pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses;                                                                                            // for global map visualization
    downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
    downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
    downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);

    // void loopClosureThread()
    // {
    //     if (loopClosureEnableFlag == false)
    //         return;

    //     ros::Rate rate(loopClosureFrequency);
    //     while (ros::ok())
    //     {
    //         rate.sleep();
    //         performLoopClosure();
    //         visualizeLoopClosure();
    //     }
    // }

    // void loopInfoHandler(const std_msgs::Float64MultiArray::ConstPtr& loopMsg)
    // {
    //     std::lock_guard<std::mutex> lock(mtxLoopInfo);
    //     if (loopMsg->data.size() != 2)
    //         return;

    //     loopInfoVec.push_back(*loopMsg);

    //     while (loopInfoVec.size() > 5)
    //         loopInfoVec.pop_front();
    // }

    // void performLoopClosure()
    // {
    //     if (cloudKeyPoses3D->points.empty() == true)
    //         return;

    //     mtx.lock();
    //     *copy_cloudKeyPoses3D = *cloudKeyPoses3D;
    //     *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
    //     mtx.unlock();

    //     // find keys
    //     int loopKeyCur;
    //     int loopKeyPre;
    //     if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
    //         if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
    //             return;

    //     // extract cloud
    //     pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
    //     pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
    //     {
    //         loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
    //         loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
    //         if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
    //             return;
    //         if (pubHistoryKeyFrames.getNumSubscribers() != 0)
    //             publishCloud(pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, name +"/"+ odometryFrame);
    //     }

    //     // ICP Settings
    //     static pcl::IterativeClosestPoint<PointType, PointType> icp;
    //     icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
    //     icp.setMaximumIterations(100);
    //     icp.setTransformationEpsilon(1e-6);
    //     icp.setEuclideanFitnessEpsilon(1e-6);
    //     icp.setRANSACIterations(0);

    //     // Align clouds
    //     icp.setInputSource(cureKeyframeCloud);
    //     icp.setInputTarget(prevKeyframeCloud);
    //     pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
    //     icp.align(*unused_result);

    //     if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
    //         return;

    //     // publish corrected cloud
    //     if (pubIcpKeyFrames.getNumSubscribers() != 0)
    //     {
    //         pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
    //         pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
    //         publishCloud(pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, name +"/"+ odometryFrame);
    //     }

    //     // Get pose transformation
    //     float x, y, z, roll, pitch, yaw;
    //     Eigen::Affine3f correctionLidarFrame;
    //     correctionLidarFrame = icp.getFinalTransformation();
    //     // transform from world origin to wrong pose
    //     Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
    //     // transform from world origin to corrected pose
    //     Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
    //     pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
    //     gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
    //     gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
    //     gtsam::Vector Vector6(6);
    //     float noiseScore = icp.getFitnessScore();
    //     Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
    //     noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);

    //     // Add pose constraint
    //     mtx.lock();
    //     loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
    //     loopPoseQueue.push_back(poseFrom.between(poseTo));
    //     loopNoiseQueue.push_back(constraintNoise);
    //     mtx.unlock();

    //     // add loop constriant
    //     loopIndexContainer[loopKeyCur] = loopKeyPre;
    // }

    // bool detectLoopClosureDistance(int *latestID, int *closestID)
    // {
    //     int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
    //     int loopKeyPre = -1;

    //     // check loop constraint added before
    //     auto it = loopIndexContainer.find(loopKeyCur);
    //     if (it != loopIndexContainer.end())
    //         return false;

    //     // find the closest history key frame
    //     std::vector<int> pointSearchIndLoop;
    //     std::vector<float> pointSearchSqDisLoop;
    //     kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
    //     kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);

    //     for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
    //     {
    //         int id = pointSearchIndLoop[i];
    //         if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
    //         {
    //             loopKeyPre = id;
    //             break;
    //         }
    //     }

    //     if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
    //         return false;

    //     *latestID = loopKeyCur;
    //     *closestID = loopKeyPre;

    //     return true;
    // }

    // bool detectLoopClosureExternal(int *latestID, int *closestID)
    // {
    //     // this function is not used yet, please ignore it
    //     int loopKeyCur = -1;
    //     int loopKeyPre = -1;

    //     std::lock_guard<std::mutex> lock(mtxLoopInfo);
    //     if (loopInfoVec.empty())
    //         return false;

    //     double loopTimeCur = loopInfoVec.front().data[0];
    //     double loopTimePre = loopInfoVec.front().data[1];
    //     loopInfoVec.pop_front();

    //     if (abs(loopTimeCur - loopTimePre) < historyKeyframeSearchTimeDiff)
    //         return false;

    //     int cloudSize = copy_cloudKeyPoses6D->size();
    //     if (cloudSize < 2)
    //         return false;

    //     // latest key
    //     loopKeyCur = cloudSize - 1;
    //     for (int i = cloudSize - 1; i >= 0; --i)
    //     {
    //         if (copy_cloudKeyPoses6D->points[i].time >= loopTimeCur)
    //             loopKeyCur = round(copy_cloudKeyPoses6D->points[i].intensity);
    //         else
    //             break;
    //     }

    //     // previous key
    //     loopKeyPre = 0;
    //     for (int i = 0; i < cloudSize; ++i)
    //     {
    //         if (copy_cloudKeyPoses6D->points[i].time <= loopTimePre)
    //             loopKeyPre = round(copy_cloudKeyPoses6D->points[i].intensity);
    //         else
    //             break;
    //     }

    //     if (loopKeyCur == loopKeyPre)
    //         return false;

    //     auto it = loopIndexContainer.find(loopKeyCur);
    //     if (it != loopIndexContainer.end())
    //         return false;

    //     *latestID = loopKeyCur;
    //     *closestID = loopKeyPre;

    //     return true;
    // }

    // void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
    // {
    //     // extract near keyframes
    //     nearKeyframes->clear();
    //     int cloudSize = copy_cloudKeyPoses6D->size();
    //     for (int i = -searchNum; i <= searchNum; ++i)
    //     {
    //         int keyNear = key + i;
    //         if (keyNear < 0 || keyNear >= cloudSize )
    //             continue;
    //         *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);
    //         *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear],   &copy_cloudKeyPoses6D->points[keyNear]);
    //     }

    //     if (nearKeyframes->empty())
    //         return;

    //     // downsample near keyframes
    //     pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
    //     downSizeFilterICP.setInputCloud(nearKeyframes);
    //     downSizeFilterICP.filter(*cloud_temp);
    //     *nearKeyframes = *cloud_temp;
    // }

    // void visualizeLoopClosure()
    // {
    //     if (loopIndexContainer.empty())
    //         return;

    //     visualization_msgs::MarkerArray markerArray;
    //     // loop nodes
    //     visualization_msgs::Marker markerNode;
    //     markerNode.header.frame_id = name +"/"+ odometryFrame;
    //     markerNode.header.stamp = timeLaserInfoStamp;
    //     markerNode.action = visualization_msgs::Marker::ADD;
    //     markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
    //     markerNode.ns = "loop_nodes";
    //     markerNode.id = 0;
    //     markerNode.pose.orientation.w = 1;
    //     markerNode.scale.x = 0.3; markerNode.scale.y = 0.3; markerNode.scale.z = 0.3;
    //     markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1;
    //     markerNode.color.a = 1;
    //     // loop edges
    //     visualization_msgs::Marker markerEdge;
    //     markerEdge.header.frame_id = name +"/"+ odometryFrame;
    //     markerEdge.header.stamp = timeLaserInfoStamp;
    //     markerEdge.action = visualization_msgs::Marker::ADD;
    //     markerEdge.type = visualization_msgs::Marker::LINE_LIST;
    //     markerEdge.ns = "loop_edges";
    //     markerEdge.id = 1;
    //     markerEdge.pose.orientation.w = 1;
    //     markerEdge.scale.x = 0.1;
    //     markerEdge.color.r = 0.9; markerEdge.color.g = 0.9; markerEdge.color.b = 0;
    //     markerEdge.color.a = 1;

    //     for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it)
    //     {
    //         int key_cur = it->first;
    //         int key_pre = it->second;
    //         geometry_msgs::Point p;
    //         p.x = copy_cloudKeyPoses6D->points[key_cur].x;
    //         p.y = copy_cloudKeyPoses6D->points[key_cur].y;
    //         p.z = copy_cloudKeyPoses6D->points[key_cur].z;
    //         markerNode.points.push_back(p);
    //         markerEdge.points.push_back(p);
    //         p.x = copy_cloudKeyPoses6D->points[key_pre].x;
    //         p.y = copy_cloudKeyPoses6D->points[key_pre].y;
    //         p.z = copy_cloudKeyPoses6D->points[key_pre].z;
    //         markerNode.points.push_back(p);
    //         markerEdge.points.push_back(p);
    //     }

    //     markerArray.markers.push_back(markerNode);
    //     markerArray.markers.push_back(markerEdge);
    //     pubLoopConstraintEdge.publish(markerArray);
    // }

    // 使用IMU进行初始化估计
    void updateInitialGuess()
    {
        // save current transformation before any processing
        // 将当前的坐标变换保存为一个Affine3f类型的变量incrementalOdometryAffineFront。在处理之前被保存下来，以便在之后的处理中进行使用。
        incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);

        static Eigen::Affine3f lastImuTransformation;
        // initialization检查KeyposesCloud3D是否为空，如果为空，那么就将imu的初始值作为初始值。
        if (dm.getLocalKeyposesCloud3D()->empty())
        { // 初始位姿由IMU磁力计提供（9轴IMU的用处）
            transformTobeMapped[0] = cloudInfo.imuRollInit;
            transformTobeMapped[1] = cloudInfo.imuPitchInit;
            transformTobeMapped[2] = cloudInfo.imuYawInit;
            // 可以选择不用9轴IMU磁力计的yaw角初始化姿态

            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;
            // 将IMU磁力计提供的欧拉角＋平移置0转换为Eigen::Affine3f格式的静态变量lastImuTransformation
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
                                                                                                                                          /* 第一帧的初始位姿估计执行至此就完成了【更新初值估计】的操作了，
                                                                                                                                            就可以退出updateInitialGuess()函数了
                                                                                                                                         */
            return;
        }

        // use imu pre-integration estimation for pose guess惯性测量单元（IMU）的预积分估计来提供姿势猜测
        // 标记上一个IMU预积分变换是否可用
        static bool lastImuPreTransAvailable = false;
        // 用于储存上一个IMU预积分的变换结果。
        static Eigen::Affine3f lastImuPreTransformation;
        // 如果里程计可用
        if (cloudInfo.odomAvailable == true)
        {
            // 获取上一帧的Transformation点云的6d位姿信息
            Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
                                                               cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
            // 如果上一个IMU预积分变换不可用，那么就将上一个IMU预积分变换置为当前的变换
            if (lastImuPreTransAvailable == false)
            {
                lastImuPreTransformation = transBack;
                lastImuPreTransAvailable = true;
            }
            // 否则
            // 获取里程计相对于上一次的里程计增量transIncre
            // 上一帧到现在的位姿变换
            else
            {
                Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
                // 将上一帧优化结束获得的关键帧估计结果transformTobeMapped转换为Eigen::Affine3f格式的transTobe
                Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
                // Twlk^ = Twlk-1 * Tlk-1lk
                Eigen::Affine3f transFinal = transTobe * transIncre;
                // 将Eigen::Affine3f格式的transFinal转换为数组形式
                pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                                  transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
                // 【当前】IMU递推的里程计 -> lastVinsTransformation（新旧交替）

                lastImuPreTransformation = transBack;
                // 仍然需要把IMU磁力计提供的姿态转换为Eigen::Affine3f格式的lastImuTransformation
                lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
                return;
            }
        }

        // use imu incremental estimation for pose guess (only rotation)
        // 如果没有里程记信息，就是用imu的旋转信息来更新，因为单纯使用imu无法得到靠谱的平移信息，因此，平移直接置0
        if (cloudInfo.imuAvailable == true)
        {
            // ROS_INFO("Using IMU initial guess");
            // 获取【最新的】IMU磁力计提供的位姿transBack
            Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
            // 获取IMU旋转变化量，当前和上一帧的imu位姿
            Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
            // 将上一帧优化结束获取的最优位姿估计值transformTobeMapped转换为Eigen::Affine3f的transTobe
            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            // Twlk^ = Twlk-1 * Tlk-1lk
            Eigen::Affine3f transFinal = transTobe * transIncre;
            // 当前帧初始估计结果transFinal再转换赋值给transFinal
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                              transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

            // 仍然记录本次IMU磁力计提供的欧拉角存至lastImuTransformation
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
            // 当前帧初始位姿估计结果已定，return退出updateInitialGuess()函数
            return;
        }
    }

    // void extractForLoopClosure()
    // {
    //     pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());
    //     int numPoses = cloudKeyPoses3D->size();
    //     for (int i = numPoses-1; i >= 0; --i)
    //     {
    //         if ((int)cloudToExtract->size() <= surroundingKeyframeSize)
    //             cloudToExtract->push_back(cloudKeyPoses3D->points[i]);
    //         else
    //             break;
    //     }

    //     extractCloud(cloudToExtract);
    // }

    // 构建局部地图
    void extractNearby()
    {
        // 定义存储【当前帧】附近(指定半径范围内)的【关键帧位置信息】的点云指针(集合)，当前帧内50米内，并降采样
        pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
        // 定义存储临近关键帧在cloudKeyPoses3D中的索引的vector
        std::vector<int> pointSearchInd;
        std::vector<float> pointSearchSqDis;

        // extract all the nearby key poses and downsample them
        // 用存储关键帧位置信息的点云/集合，用以构建KD-Tree
        kdtreeSurroundingKeyPoses->setInputCloud(dm.getLocalKeyposesCloud3D()); // create kd-tree
                                                                                /* 根据最新关键帧位置dm.getLocalKeyposesCloud3D()提取关键帧位置集合cloudKeyPoses3D中
                                                                                 距离最后一个关键帧指定半径范围(50.0m)内的所有关键帧
                                                                                 radiusSearch 找到的关键帧
                                                                                 在cloudKeyPoses3D中的索引存至pointSearchInd
                                                                                 距离最新关键帧距离的平方存至pointSearchSqDis
                                                                              */
        kdtreeSurroundingKeyPoses->radiusSearch(dm.getLocalKeyposesCloud3D()->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
        // 遍历存储索引的vector pointSearchInd
        for (int i = 0; i < (int)pointSearchInd.size(); ++i)
        {
            // 获取当前遍历到的关键帧在cloudKeyPoses3D中的索引id
            int index = pointSearchInd[i];
            // 将该关键帧存至surroundingKeyPoses
            surroundingKeyPoses->push_back(dm.getLocalKeyposesCloud3D()->points[index]);
        }

        /* 避免50m范围内关键帧过密,对提取出的临近关键帧降采样，每2*2*2m^3范围选出一个关键帧代表，
    得到surroundingKeyPosesDS
    ToDO
*/
        downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
        downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
        // for循环遍历，每次都是用pt变量，根据每个点的最近邻点的强度信息，更新该点的强度属性。
        for (auto &pt : surroundingKeyPosesDS->points)
        {
            // kdtreeSurroundingKeyPoses是一个包含周围关键位置点的KD树，可以用来进行快速的最近邻搜索。
            // 找到与当前点pt最近的一个点的索引存至pointSearchInd，将结果存在pointSearchSqDis中
            kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
            // 获取points中最近邻点的索引将强度信息赋值给pt.intensity
            pt.intensity = dm.getLocalKeyposesCloud3D()->points[pointSearchInd[0]].intensity;
        }

        // also extract some latest key frames in case the robot rotates in one position
        // 上面的surroundingKeyPosesDS只能针对【位置】上选出邻近范围内的关键帧
        // 如果出现原地旋转的情况 则这些姿态不同的关键帧只能根据位置选一个
        // 为了尽量选出姿态不同的关键帧进入surroundingKeyPosesDS，
        int numPoses = dm.getLocalKeyposesCloud3D()->size();
        // 从新往旧遍历关键帧集合
        for (int i = numPoses - 1; i >= 0; --i)
        {
            // 选出距离当前帧点云时间timeLaserInfoCur小于10s的关键帧也塞入surroundingKeyPosesDS
            if (timeLaserInfoCur - dm.getLocalKeyposesCloud6D()->points[i].time < 10.0)
                surroundingKeyPosesDS->push_back(dm.getLocalKeyposesCloud3D()->points[i]);
            // 超过10s后直接break结束遍历循环
            else
                break;
        }

        // 根据筛选出来的近邻关键帧surroundingKeyPosesDS进行局部地图构建
        /* surroundingKeyPosesDS只是一个关键帧位置集合，
           要得到局部地图还要整合其对应点云
           得到laserCloudCornerFromMap、laserCloudSurfFromMap
        */
        extractCloud(surroundingKeyPosesDS);
    }

    // 局部地图构建,也就是按照索引的位姿拼接关键帧
    void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
    {
        // fuse the map
        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear();
        // 遍历【筛选出来的关键帧集合】cloudToExtract
        for (int i = 0; i < (int)cloudToExtract->size(); ++i)
        {

            if (pointDistance(cloudToExtract->points[i], dm.getLocalKeyposesCloud3D()->back()) > surroundingKeyframeSearchRadius)
                // 距离当前帧位置超过50m的关键帧不予考虑
                continue;
            // 获取遍历到的关键帧索引thisKeyInd(intensity)
            int thisKeyInd = (int)cloudToExtract->points[i].intensity;
            // cornerCloudKeyFrames是在 saveKeyFramesAndFactor中被赋值
            if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end())
            {
                // transformed cloud available
                // 将遍历到的关键帧按照其【位姿】转到世界坐标系，加入到点云vector中，
                *laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
                *laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
            }
            else
            {
                // transformed cloud not available点云的转换并将转换后的点云加入到对应的点云向量中，并存储到容器中以备下次查找使用。
                pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &dm.getLocalKeyposesCloud6D()->points[thisKeyInd]);
                pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &dm.getLocalKeyposesCloud6D()->points[thisKeyInd]);
                *laserCloudCornerFromMap += laserCloudCornerTemp;
                *laserCloudSurfFromMap += laserCloudSurfTemp;
                laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
            }
        }

        // 对局部地图进行降采样
        // Downsample the surrounding corner key frames (or map)
        downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
        downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
        laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
        // Downsample the surrounding surf key frames (or map)
        downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
        laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();

        // clear map cache if too large如果局部地图过大的话进行清理
        if (laserCloudMapContainer.size() > 1000)
            laserCloudMapContainer.clear();
    }

    // 根据是否是第一帧，判断是否需要构建局部地图
    void extractSurroundingKeyFrames()
    {
        // 局部关键点云位姿是否为空
        if (dm.getLocalKeyposesCloud3D()->empty() == true)
            return;

        // if (loopClosureEnableFlag == true)
        // {
        //     extractForLoopClosure();
        // } else {
        //     extractNearby();
        // }

        extractNearby();
    }

    // 降采样当前帧点云
    void downsampleCurrentScan()
    {
        // Downsample cloud from current scan
        laserCloudCornerLastDS->clear();
        downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
        downSizeFilterCorner.filter(*laserCloudCornerLastDS);
        laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();

        laserCloudSurfLastDS->clear();
        downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
        downSizeFilterSurf.filter(*laserCloudSurfLastDS);
        laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
    }

    void updatePointAssociateToMap()
    {
        transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
    }

    // 点线特征处理
    void cornerOptimization()
    {
        /* 前面在updateInitialGuess()函数中已经得到当前帧Twl的预测值transformTobeMapped了，
           现在需要将其转换为Eigen::Affine3f格式的transPointAssociateToMap
        */
        updatePointAssociateToMap();
// 接下来的这个for循环将被多个线程同时运行的，也就是多个线程同时运行一个for循环
// 并行执行for循环
#pragma omp parallel for num_threads(numberOfCores)
        // 遍历当前帧降采样后的角点云
        for (int i = 0; i < laserCloudCornerLastDSNum; i++)
        {
            PointType pointOri, pointSel, coeff;
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;
            // 取出降采样后的当前帧角点云中 遍历到的第i个点pointOri
            pointOri = laserCloudCornerLastDS->points[i];
            // 按照当前帧预测的位姿transPointAssociateToMap将pointOri转换到W'系，得到pointSel

            pointAssociateToMap(&pointOri, &pointSel);
            // 在构建的角点局部地图laserCloudCornerFromMapDS中找到距离pointSel最近的5个点

            kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

            cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
            cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
            cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));

            if (pointSearchSqDis[4] < 1.0)
            {
                float cx = 0, cy = 0, cz = 0;
                for (int j = 0; j < 5; j++)
                {
                    cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                    cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                    cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                }
                cx /= 5;
                cy /= 5;
                cz /= 5;
                // 计算这5个点的协方差矩阵matA1
                float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                for (int j = 0; j < 5; j++)
                {
                    float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                    float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                    float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;

                    a11 += ax * ax;
                    a12 += ax * ay;
                    a13 += ax * az;
                    a22 += ay * ay;
                    a23 += ay * az;
                    a33 += az * az;
                }
                a11 /= 5;
                a12 /= 5;
                a13 /= 5;
                a22 /= 5;
                a23 /= 5;
                a33 /= 5;

                matA1.at<float>(0, 0) = a11;
                matA1.at<float>(0, 1) = a12;
                matA1.at<float>(0, 2) = a13;
                matA1.at<float>(1, 0) = a12;
                matA1.at<float>(1, 1) = a22;
                matA1.at<float>(1, 2) = a23;
                matA1.at<float>(2, 0) = a13;
                matA1.at<float>(2, 1) = a23;
                matA1.at<float>(2, 2) = a33;

                cv::eigen(matA1, matD1, matV1);

                if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1))
                {

                    float x0 = pointSel.x;
                    float y0 = pointSel.y;
                    float z0 = pointSel.z;
                    float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                    float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                    float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                    float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                    float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                    float z2 = cz - 0.1 * matV1.at<float>(0, 2);

                    float a012 = sqrt(((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) + ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)));

                    float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));

                    float la = ((y1 - y2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + (z1 - z2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))) / a012 / l12;

                    float lb = -((x1 - x2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) - (z1 - z2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;

                    float lc = -((x1 - x2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) + (y1 - y2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;

                    float ld2 = a012 / l12;

                    float s = 1 - 0.9 * fabs(ld2);

                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2;

                    if (s > 0.1)
                    {
                        laserCloudOriCornerVec[i] = pointOri;
                        coeffSelCornerVec[i] = coeff;
                        laserCloudOriCornerFlag[i] = true;
                    }
                }
            }
        }
    }

    void surfOptimization()
    {
        updatePointAssociateToMap();

#pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < laserCloudSurfLastDSNum; i++)
        {
            PointType pointOri, pointSel, coeff;
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;

            pointOri = laserCloudSurfLastDS->points[i];
            pointAssociateToMap(&pointOri, &pointSel);
            kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

            Eigen::Matrix<float, 5, 3> matA0;
            Eigen::Matrix<float, 5, 1> matB0;
            Eigen::Vector3f matX0;

            matA0.setZero();
            matB0.fill(-1);
            matX0.setZero();

            if (pointSearchSqDis[4] < 1.0)
            {
                for (int j = 0; j < 5; j++)
                {
                    matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                    matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                    matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                }

                matX0 = matA0.colPivHouseholderQr().solve(matB0);

                float pa = matX0(0, 0);
                float pb = matX0(1, 0);
                float pc = matX0(2, 0);
                float pd = 1;

                float ps = sqrt(pa * pa + pb * pb + pc * pc);
                pa /= ps;
                pb /= ps;
                pc /= ps;
                pd /= ps;

                bool planeValid = true;
                for (int j = 0; j < 5; j++)
                {
                    if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                             pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                             pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2)
                    {
                        planeValid = false;
                        break;
                    }
                }

                if (planeValid)
                {
                    float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

                    float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z));

                    coeff.x = s * pa;
                    coeff.y = s * pb;
                    coeff.z = s * pc;
                    coeff.intensity = s * pd2;

                    if (s > 0.1)
                    {
                        laserCloudOriSurfVec[i] = pointOri;
                        coeffSelSurfVec[i] = coeff;
                        laserCloudOriSurfFlag[i] = true;
                    }
                }
            }
        }
    }

    void combineOptimizationCoeffs()
    {
        // combine corner coeffs
        // 遍历局部地图所有角点
        for (int i = 0; i < laserCloudCornerLastDSNum; ++i)
        {
            if (laserCloudOriCornerFlag[i] == true)
            {
                laserCloudOri->push_back(laserCloudOriCornerVec[i]);
                coeffSel->push_back(coeffSelCornerVec[i]);
            }
        }
        // combine surf coeffs
        for (int i = 0; i < laserCloudSurfLastDSNum; ++i)
        {
            if (laserCloudOriSurfFlag[i] == true)
            {
                laserCloudOri->push_back(laserCloudOriSurfVec[i]);
                coeffSel->push_back(coeffSelSurfVec[i]);
            }
        }
        // reset flag for next iteration
        // 将判断线、面特征分布是否合理的标志变量vector全部置为false
        /* 注：
            目前在执行的函数是combineOptimizationCoeffs()，
            在执行完combineOptimizationCoeffs()之后，会执行LMOptimization()。
            而LMOptimization()构建增量方程用到的约束个数就是由laserCloudOri的数量决定的。
            而只有在laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]为true才会将点塞入laserCloudOri。
            而laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]
               是在cornerOptimization()和surfOptimization()中判断s > 0.1时才置为true。

            即
            1.cornerOptimization()和surfOptimization() （都是在scan2MapOptimization()函数中）
              s > 0.1 （并不是所有角点和面点都满足这个条件）
              laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]置为true
            2.combineOptimizationCoeffs()
              分别遍历角点和面点，
              只记录laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]置为true、即满足s > 0.1的角点和面点
               对应的特征点和残差分别塞入laserCloudOri、coeffSel
              即执行完该函数laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]作用就失效了，
              一朝 当前帧点云 一朝 laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]
              因此，为了下帧点云使用，需要提前将laserCloudOriCornerFlag[i]、laserCloudOriSurfFlag[i]置为false

            3.LMOptimization()
              求雅克比和残差用到的都在laserCloudOri和coeffSel中取
        */
        std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
    }

    bool LMOptimization(int iterCount)
    {
        // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
        // lidar <- camera      ---     camera <- lidar
        // x = z                ---     x = y
        // y = x                ---     y = z
        // z = y                ---     z = x
        // roll = yaw           ---     roll = pitch
        // pitch = roll         ---     pitch = yaw
        // yaw = pitch          ---     yaw = roll

        // lidar -> camera
        float srx = sin(transformTobeMapped[1]);
        float crx = cos(transformTobeMapped[1]);
        float sry = sin(transformTobeMapped[2]);
        float cry = cos(transformTobeMapped[2]);
        float srz = sin(transformTobeMapped[0]);
        float crz = cos(transformTobeMapped[0]);

        int laserCloudSelNum = laserCloudOri->size();
        // 确保可以构建增量方程的有效观测超过50个
        // 不满足的话直接退出LMOptimization
        if (laserCloudSelNum < 50)
        {
            return false;
        }
        // √[Σ^(-1)]J
        // 【公式推导】
        cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
        cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
        cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
        cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
        cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
        cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));

        PointType pointOri, coeff;
        // 构造H△x=b
        // 遍历存储【当前帧】角/面特征的点云laserCloudOri中laserCloudSelNum个点
        for (int i = 0; i < laserCloudSelNum; i++)
        {
            // 求解遍历到的LiDAR系下的点laserCloudOri、残差coeffSel
            // 在camera系下的点坐标pointOri以及残差coeff
            // lidar -> camera
            pointOri.x = laserCloudOri->points[i].y;
            pointOri.y = laserCloudOri->points[i].z;
            pointOri.z = laserCloudOri->points[i].x;
            // lidar -> camera
            coeff.x = coeffSel->points[i].y;
            coeff.y = coeffSel->points[i].z;
            coeff.z = coeffSel->points[i].x;
            coeff.intensity = coeffSel->points[i].intensity;
            // in camera
            // arx = ∂e/∂rxc = (∂e/∂pw)*(∂pw/∂rxc)
            float arx = (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y - srx * sry * pointOri.z) * coeff.x + (-srx * srz * pointOri.x - crz * srx * pointOri.y - crx * pointOri.z) * coeff.y + (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y - cry * srx * pointOri.z) * coeff.z;

            float ary = ((cry * srx * srz - crz * sry) * pointOri.x + (sry * srz + cry * crz * srx) * pointOri.y + crx * cry * pointOri.z) * coeff.x + ((-cry * crz - srx * sry * srz) * pointOri.x + (cry * srz - crz * srx * sry) * pointOri.y - crx * sry * pointOri.z) * coeff.z;

            float arz = ((crz * srx * sry - cry * srz) * pointOri.x + (-cry * crz - srx * sry * srz) * pointOri.y) * coeff.x + (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y + ((sry * srz + cry * crz * srx) * pointOri.x + (crz * sry - cry * srx * srz) * pointOri.y) * coeff.z;
            // camera -> lidar
            // √[Σ^(-1)]J
            // 1/σi JiL = 1/σi ∂e/∂rxL = 1/σi ∂e/∂rzc
            matA.at<float>(i, 0) = arz;
            matA.at<float>(i, 1) = arx;
            matA.at<float>(i, 2) = ary;
            matA.at<float>(i, 3) = coeff.z;
            matA.at<float>(i, 4) = coeff.x;
            matA.at<float>(i, 5) = coeff.y;
            matB.at<float>(i, 0) = -coeff.intensity;
        }

        cv::transpose(matA, matAt);
        matAtA = matAt * matA;
        matAtB = matAt * matB;
        cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
        // 如果是第一次迭代，判断JT[Σ^(-1)]J的最小特征值
        if (iterCount == 0)
        {

            cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
            // 求解JT[Σ^(-1)]J的特征值填入matE，特征值填入matV
            cv::eigen(matAtA, matE, matV);
            matV.copyTo(matV2);

            isDegenerate = false;
            float eignThre[6] = {100, 100, 100, 100, 100, 100};
            for (int i = 5; i >= 0; i--)
            {
                if (matE.at<float>(0, i) < eignThre[i])
                {
                    for (int j = 0; j < 6; j++)
                    {
                        matV2.at<float>(i, j) = 0;
                    }
                    isDegenerate = true;
                }
                else
                {
                    break;
                }
            }
            matP = matV.inv() * matV2;
        }

        if (isDegenerate)
        {
            cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
            matX.copyTo(matX2);
            matX = matP * matX2;
        }

        transformTobeMapped[0] += matX.at<float>(0, 0);
        transformTobeMapped[1] += matX.at<float>(1, 0);
        transformTobeMapped[2] += matX.at<float>(2, 0);
        transformTobeMapped[3] += matX.at<float>(3, 0);
        transformTobeMapped[4] += matX.at<float>(4, 0);
        transformTobeMapped[5] += matX.at<float>(5, 0);
        // 本次迭代旋转变化量
        float deltaR = sqrt(
            pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
            pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
            pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
        // 迭代的位移变化量
        float deltaT = sqrt(
            pow(matX.at<float>(3, 0) * 100, 2) +
            pow(matX.at<float>(4, 0) * 100, 2) +
            pow(matX.at<float>(5, 0) * 100, 2));

        if (deltaR < 0.05 && deltaT < 0.05)
        {
            return true; // converged
        }
        return false; // keep optimizing
    }

    // 优化当前帧位姿
    void scan2MapOptimization()
    {
        // 如果当前帧是第一帧，直接返回
        if (dm.getLocalKeyposesCloud3D()->points.empty())
            return;
        // 只有当前帧角点云、面点云特征分别大于10、100,才能进行scan-to-map优化
        if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
        {
            // 为当前帧角点云、面点云特征分别建立kd树
            kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
            kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

            // 迭代优化
            for (int iterCount = 0; iterCount < 30; iterCount++)
            {
                laserCloudOri->clear();
                coeffSel->clear();

                cornerOptimization();
                surfOptimization();

                combineOptimizationCoeffs();

                if (LMOptimization(iterCount) == true)
                    break;
            }

            transformUpdate();
        }
        else
        {
            ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
        }
    }

    void transformUpdate()
    {
        if (cloudInfo.imuAvailable == true)
        {
            if (std::abs(cloudInfo.imuPitchInit) < 1.4)
            {
                double imuWeight = imuRPYWeight;
                tf::Quaternion imuQuaternion;
                tf::Quaternion transformQuaternion;
                double rollMid, pitchMid, yawMid;

                // slerp roll
                transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
                imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
                tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                transformTobeMapped[0] = rollMid;

                // slerp pitch
                transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
                imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
                tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                transformTobeMapped[1] = pitchMid;
            }
        }

        transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
        transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
        transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);

        incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
    }

    float constraintTransformation(float value, float limit)
    {
        if (value < -limit)
            value = -limit;
        if (value > limit)
            value = limit;

        return value;
    }

    bool saveFrame()
    {
        // 如果当前是第一帧，自然是关键帧（返回true）
        if (cloudKeyPoses3D->points.empty())
            return true;
        //判断 传感器是否为livox 
        if (sensor == SensorType::LIVOX)
        {
            if (timeLaserInfoCur - cloudKeyPoses6D->back().time > 1.0)
                return true;
        }
        // 计算最新关键帧的位姿//存储所有关键帧位姿的点云树结构，back表示最新的
        Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
        // 计算目前帧（不一定是关键帧）
        Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                                            transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
        // 两帧之间的位姿变换 Eigen::Affine3f格式
        Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
        float x, y, z, roll, pitch, yaw;
        // 用于获取点云中的平移和欧拉角信息
        pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);

        // r p y 或者距离中任何一个超过阈值 就是关键帧 返回true
        // surroundingkeyframeAddingAngleThreshold： 0.2rad 0.2*57.3 = 11.5°
        // surroundingkeyframeAddingDistThreshold：1m
        if (abs(roll)  < surroundingkeyframeAddingAngleThreshold &&
            abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
            abs(yaw)   < surroundingkeyframeAddingAngleThreshold &&
            sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
            return false;

        return true;
    }

    //加入里程计因子
    void addOdomFactor()
    {
        // 如果是第一帧，就加入先验因子
        if (cloudKeyPoses3D->points.empty())
        {
            noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
            gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
            initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
        }else{
            noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
            gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
            gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);
            gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
            initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
        }
    }

 /*    void addGPSFactor()
    {
        if (gpsQueue.empty())
            return;

        // wait for system initialized and settles down
        if (cloudKeyPoses3D->points.empty())
            return;
        else
        {
            if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
                return;
        }

        // pose covariance small, no need to correct
        if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)
            return;

        // last gps position
        static PointType lastGPSPoint;

        while (!gpsQueue.empty())
        {
            if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
            {
                // message too old
                gpsQueue.pop_front();
            }
            else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2)
            {
                // message too new
                break;
            }
            else
            {
                nav_msgs::Odometry thisGPS = gpsQueue.front();
                gpsQueue.pop_front();

                // GPS too noisy, skip
                float noise_x = thisGPS.pose.covariance[0];
                float noise_y = thisGPS.pose.covariance[7];
                float noise_z = thisGPS.pose.covariance[14];
                if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
                    continue;

                float gps_x = thisGPS.pose.pose.position.x;
                float gps_y = thisGPS.pose.pose.position.y;
                float gps_z = thisGPS.pose.pose.position.z;
                if (!useGpsElevation)
                {
                    gps_z = transformTobeMapped[5];
                    noise_z = 0.01;
                }

                // GPS not properly initialized (0,0,0)
                if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
                    continue;

                // Add GPS every a few meters
                PointType curGPSPoint;
                curGPSPoint.x = gps_x;
                curGPSPoint.y = gps_y;
                curGPSPoint.z = gps_z;
                if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
                    continue;
                else
                    lastGPSPoint = curGPSPoint;

                gtsam::Vector Vector3(3);
                Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);
                noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
                gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
                gtSAMgraph.add(gps_factor);

                aLoopIsClosed = true;
                break;
            }
        }
    }
 */
    
    //加入回环因子
    void addLoopFactor()
    {
        // 判断loopIndexQueue循环检测队列是否为空
        if (loopIndexQueue.empty())
            return;

        for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
        {
            // 取出当前帧的索引（新
            int indexFrom = loopIndexQueue[i].first;
            // 取出历史帧的索引（旧
            int indexTo = loopIndexQueue[i].second;
            // 获取【历史帧】到【新帧】的位姿约束
            // vector<gtsam::Pose3> loopPoseQueue;存贮gtsam位姿
            gtsam::Pose3 poseBetween = loopPoseQueue[i];
            // 获取位姿约束的噪声
            gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
            gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
        }

        loopIndexQueue.clear();
        loopPoseQueue.clear();
        loopNoiseQueue.clear();
        aLoopIsClosed = true;
    }
    
    void saveKeyFramesAndFactor()
    {
    // 参数标志位 是否保存关键帧
        if (saveFrame() == false)
            return;

        // odom factor
        addOdomFactor();

        // gps factor
        // addGPSFactor();

        // loop factor
        addLoopFactor();

        // cout << "****************************************************" << endl;
        // gtSAMgraph.print("GTSAM Graph:\n");

        // update iSAM
        isam->update(gtSAMgraph, initialEstimate);
        isam->update();

        // 是否进行ISAM更新优化位姿
        if (aLoopIsClosed == true)
        {
            isam->update();
            isam->update();
            isam->update();
            isam->update();
            isam->update();
        }

        gtSAMgraph.resize(0);
        initialEstimate.clear();

        //save key poses保存关键帧位姿
        PointType thisPose3D;
        PointTypePose thisPose6D;
        // 创造变量latestEstimate存储最新的位姿估计结果
        Pose3 Pose3 latestEstimate;;
        // 从isam中取出最新的优化结果,并且将结果存储在isamCurrentEstimate中
        isamCurrentEstimate = isam->calculateEstimate();
        // 从优化结果isamCurrentEstimate取出最新的位姿估计结果latestEstimate
        latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
        // cout << "****************************************************" << endl;
        // isamCurrentEstimate.print("Current estimate: ");

        thisPose3D.x = latestEstimate.translation().x();
        thisPose3D.y = latestEstimate.translation().y();
        thisPose3D.z = latestEstimate.translation().z();
        thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
        cloudKeyPoses3D->push_back(thisPose3D);

        thisPose6D.x = thisPose3D.x;
        thisPose6D.y = thisPose3D.y;
        thisPose6D.z = thisPose3D.z;
        thisPose6D.intensity = thisPose3D.intensity ; // this can be used as index
        // 获取最新估计的位姿中的欧拉角
        thisPose6D.roll  = latestEstimate.rotation().roll();
        thisPose6D.pitch = latestEstimate.rotation().pitch();
        thisPose6D.yaw   = latestEstimate.rotation().yaw();
        thisPose6D.time = timeLaserInfoCur;
        cloudKeyPoses6D->push_back(thisPose6D);

        // cout << "****************************************************" << endl;
        // cout << "Pose covariance:" << endl;
        // cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
        // 计算姿态协方差矩阵（pose covariance matrix）
        poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);

        // save updated transform 保存更新的转换
        transformTobeMapped[0] = latestEstimate.rotation().roll();
        transformTobeMapped[1] = latestEstimate.rotation().pitch();
        transformTobeMapped[2] = latestEstimate.rotation().yaw();
        transformTobeMapped[3] = latestEstimate.translation().x();
        transformTobeMapped[4] = latestEstimate.translation().y();
        transformTobeMapped[5] = latestEstimate.translation().z();

        // save all the received edge and surf points 保存所有接收到的角和面点
        pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
        pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);
        pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);

        // save key frame cloud
        cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
        surfCloudKeyFrames.push_back(thisSurfKeyFrame);

        // save path for visualization
        updatePath(thisPose6D);
    }

    void correctPoses()
    {
        // 第一帧不执行这个函数
        if (cloudKeyPoses3D->points.empty())
            return;
        // 如果检测到回环
        if (aLoopIsClosed == true)
        {
            // clear map cache 清空地图缓存
            laserCloudMapContainer.clear();
            // clear path 清空路径
            globalPath.poses.clear();
            // update key poses 
            int numPoses = isamCurrentEstimate.size();// 优化后的位姿数量
            for (int i = 0; i < numPoses; ++i)
            {
                cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
                cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
                cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();

                cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
                cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
                cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
                cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
                cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
                cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();

                // 更新关键帧路径
                updatePath(cloudKeyPoses6D->points[i]);
            }

            aLoopIsClosed = false;
        }
    }

    // 输入关键帧位姿，更新关键帧路径
    void updatePath(const PointTypePose& pose_in)
    {
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
        pose_stamped.header.frame_id = name +"/"+ odometryFrame;
        pose_stamped.pose.position.x = pose_in.x;
        pose_stamped.pose.position.y = pose_in.y;
        pose_stamped.pose.position.z = pose_in.z;
        tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
        pose_stamped.pose.orientation.x = q.x();
        pose_stamped.pose.orientation.y = q.y();
        pose_stamped.pose.orientation.z = q.z();
        pose_stamped.pose.orientation.w = q.w();

        globalPath.poses.push_back(pose_stamped);
    }

    void publishOdometry()
    {
        // transformTobeMapped为优化出来的位姿变换
        // Publish odometry for ROS (global)
        nav_msgs::Odometry laserOdometryROS;
        laserOdometryROS.header.stamp = timeLaserInfoStamp;
        laserOdometryROS.header.frame_id = name + "/" + odometryFrame;
        laserOdometryROS.child_frame_id = name + "/" + "odom_mapping";
        laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
        laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
        laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
        // 通过欧拉角的转换得出四元数
        laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
        pubLaserOdometryGlobal.publish(laserOdometryROS);

        // Publish TF
        static tf::TransformBroadcaster br;
        tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
                                                      tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
        tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, name + "/" + odometryFrame, name + "/" + "lidar_link");
        br.sendTransform(trans_odom_to_lidar);

        // Publish odometry for ROS (incremental)
        static bool lastIncreOdomPubFlag = false;
        static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg
        static Eigen::Affine3f increOdomAffine;         // incremental odometry in affine
        if (lastIncreOdomPubFlag == false)
        {
            lastIncreOdomPubFlag = true;
            laserOdomIncremental = laserOdometryROS;
            increOdomAffine = trans2Affine3f(transformTobeMapped);
        }
        else
        {
            Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
            increOdomAffine = increOdomAffine * affineIncre;
            float x, y, z, roll, pitch, yaw;
            pcl::getTranslationAndEulerAngles(increOdomAffine, x, y, z, roll, pitch, yaw);
            if (cloudInfo.imuAvailable == true)
            {
                if (std::abs(cloudInfo.imuPitchInit) < 1.4)
                {
                    double imuWeight = 0.1;
                    tf::Quaternion imuQuaternion;
                    tf::Quaternion transformQuaternion;
                    double rollMid, pitchMid, yawMid;

                    // slerp roll
                    transformQuaternion.setRPY(roll, 0, 0);
                    imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
                    tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                    roll = rollMid;

                    // slerp pitch
                    transformQuaternion.setRPY(0, pitch, 0);
                    imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
                    tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                    pitch = pitchMid;
                }
            }
            laserOdomIncremental.header.stamp = timeLaserInfoStamp;
            laserOdomIncremental.header.frame_id = name + "/" + odometryFrame;
            laserOdomIncremental.child_frame_id = name + "/" + "odom_mapping";
            laserOdomIncremental.pose.pose.position.x = x;
            laserOdomIncremental.pose.pose.position.y = y;
            laserOdomIncremental.pose.pose.position.z = z;
            laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
            if (isDegenerate)
                laserOdomIncremental.pose.covariance[0] = 1;
            else
                laserOdomIncremental.pose.covariance[0] = 0;
        }
        pubLaserOdometryIncremental.publish(laserOdomIncremental);
    }

    void publishFrames()
    {
        if (dm.robots[dm.getID()].cloudKeyPoses3D->points.empty())
            return;
        // publish key poses
        publishCloud(pubKeyPoses, dm.robots[dm.getID()].cloudKeyPoses3D, timeLaserInfoStamp, name +"/"+ odometryFrame);
        // Publish surrounding key frames
        publishCloud(pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, name +"/"+ odometryFrame);
        // publish registered key frame
        if (pubRecentKeyFrame.getNumSubscribers() != 0)
        {
            pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
            PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
            *cloudOut += *transformPointCloud(laserCloudCornerLastDS,  &thisPose6D);
            *cloudOut += *transformPointCloud(laserCloudSurfLastDS,    &thisPose6D);
            publishCloud(pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, name +"/"+ odometryFrame);
        }
        // publish registered high-res raw cloud
        if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
        {
            pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
            pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
            PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
            *cloudOut = *transformPointCloud(cloudOut,  &thisPose6D);
            publishCloud(pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, name +"/"+ odometryFrame);
        }
        // publish path
        if (pubPath.getNumSubscribers() != 0)
        {
            globalPath.header.stamp = timeLaserInfoStamp;
            globalPath.header.frame_id = name +"/"+ odometryFrame;
            pubPath.publish(globalPath);
        }
        
/*         //按照机器人编号🉑机器人的位姿变化，并且输出成三个txt文件
        int robot_id = {0；1；2 }
        //如果机器人的编号等于数组内的数字，就输出该机器人的位姿变化
        switch(dm.getID() == 

        case 0 :
             */
      
        
        
       







    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lio_sam");

    mapOptimization MO; // 构建一个mapOptimization对象MO

    ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");

    // std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
    // std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
    // 增加dm对象到loop_closure_thread和visualize_map_thread
    std::thread loop_closure_thread(&distributedMapping::loopClosureThread, &MO.dm); // 提供了一个指向distributedMapping对象MO.dm的指针作为参数传递给该函数。
    std::thread visualize_map_thread(&distributedMapping::globalMapThread, &MO.dm);

    ros::spin();

    // loopthread.join();
    // visualizeMapThread.join();
    loop_closure_thread.join();
    visualize_map_thread.join();

    return 0;
}
